/*********************************************************************
 *
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2016,
 *  TU Dortmund - Institute of Control Theory and Systems Engineering.
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of the institute nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *
 * Author: Christoph Rösmann
 *********************************************************************/

#include <teb_local_planner/optimal_planner.h>
#include <map>
#include <limits>

namespace teb_local_planner
{

// ============== Implementation ===================

TebOptimalPlanner::TebOptimalPlanner() : cfg_(NULL), obstacles_(NULL), via_points_(NULL), cost_(HUGE_VAL), prefer_rotdir_(RotType::none),
                                         robot_model_(new PointRobotFootprint()), initialized_(false), optimized_(false)
{
}

TebOptimalPlanner::TebOptimalPlanner(const TebConfig &cfg, ObstContainer *obstacles, RobotFootprintModelPtr robot_model, TebVisualizationPtr visual, const ViaPointContainer *via_points)
{
    initialize(cfg, obstacles, robot_model, visual, via_points);
}

TebOptimalPlanner::~TebOptimalPlanner()
{
    clearGraph();
    // free dynamically allocated memory
    //if (optimizer_)
    //  g2o::Factory::destroy();
    //g2o::OptimizationAlgorithmFactory::destroy();
    //g2o::HyperGraphActionLibrary::destroy();
}

void TebOptimalPlanner::initialize(const TebConfig &cfg, ObstContainer *obstacles, RobotFootprintModelPtr robot_model, TebVisualizationPtr visual, const ViaPointContainer *via_points)
{
    // init optimizer (set solver and block ordering settings)
    optimizer_ = initOptimizer();

    cfg_ = &cfg;
    obstacles_ = obstacles;
    robot_model_ = robot_model;
    via_points_ = via_points;
    cost_ = HUGE_VAL;
    prefer_rotdir_ = RotType::none;
    setVisualization(visual);

    vel_start_.first = true;
    vel_start_.second.linear.x = 0;
    vel_start_.second.linear.y = 0;
    vel_start_.second.angular.z = 0;

    vel_goal_.first = true;
    vel_goal_.second.linear.x = 0;
    vel_goal_.second.linear.y = 0;
    vel_goal_.second.angular.z = 0;
    initialized_ = true;
}

void TebOptimalPlanner::setVisualization(TebVisualizationPtr visualization)
{
    visualization_ = visualization;
}

void TebOptimalPlanner::visualize()
{
    if (!visualization_)
        return;

    visualization_->publishLocalPlanAndPoses(teb_);

    if (teb_.sizePoses() > 0)
        visualization_->publishRobotFootprintModel(teb_.Pose(0), *robot_model_);

    if (cfg_->trajectory.publish_feedback)
        visualization_->publishFeedbackMessage(*this, *obstacles_);
}

/*
 * registers custom vertices and edges in g2o framework
 */
void TebOptimalPlanner::registerG2OTypes()
{
    g2o::Factory *factory = g2o::Factory::instance();
    factory->registerType("VERTEX_POSE", new g2o::HyperGraphElementCreator<VertexPose>);
    factory->registerType("VERTEX_TIMEDIFF", new g2o::HyperGraphElementCreator<VertexTimeDiff>);

    factory->registerType("EDGE_TIME_OPTIMAL", new g2o::HyperGraphElementCreator<EdgeTimeOptimal>);
    factory->registerType("EDGE_SHORTEST_PATH", new g2o::HyperGraphElementCreator<EdgeShortestPath>);
    factory->registerType("EDGE_VELOCITY", new g2o::HyperGraphElementCreator<EdgeVelocity>);
    factory->registerType("EDGE_VELOCITY_HOLONOMIC", new g2o::HyperGraphElementCreator<EdgeVelocityHolonomic>);
    factory->registerType("EDGE_ACCELERATION", new g2o::HyperGraphElementCreator<EdgeAcceleration>);
    factory->registerType("EDGE_ACCELERATION_START", new g2o::HyperGraphElementCreator<EdgeAccelerationStart>);
    factory->registerType("EDGE_ACCELERATION_GOAL", new g2o::HyperGraphElementCreator<EdgeAccelerationGoal>);
    factory->registerType("EDGE_ACCELERATION_HOLONOMIC", new g2o::HyperGraphElementCreator<EdgeAccelerationHolonomic>);
    factory->registerType("EDGE_ACCELERATION_HOLONOMIC_START", new g2o::HyperGraphElementCreator<EdgeAccelerationHolonomicStart>);
    factory->registerType("EDGE_ACCELERATION_HOLONOMIC_GOAL", new g2o::HyperGraphElementCreator<EdgeAccelerationHolonomicGoal>);
    factory->registerType("EDGE_KINEMATICS_DIFF_DRIVE", new g2o::HyperGraphElementCreator<EdgeKinematicsDiffDrive>);
    factory->registerType("EDGE_KINEMATICS_CARLIKE", new g2o::HyperGraphElementCreator<EdgeKinematicsCarlike>);
    factory->registerType("EDGE_OBSTACLE", new g2o::HyperGraphElementCreator<EdgeObstacle>);
    factory->registerType("EDGE_INFLATED_OBSTACLE", new g2o::HyperGraphElementCreator<EdgeInflatedObstacle>);
    factory->registerType("EDGE_DYNAMIC_OBSTACLE", new g2o::HyperGraphElementCreator<EdgeDynamicObstacle>);
    factory->registerType("EDGE_VIA_POINT", new g2o::HyperGraphElementCreator<EdgeViaPoint>);
    factory->registerType("EDGE_PREFER_ROTDIR", new g2o::HyperGraphElementCreator<EdgePreferRotDir>);
    return;
}

/*
 * initialize g2o optimizer. Set solver settings here.
 * Return: pointer to new SparseOptimizer Object.
 */
boost::shared_ptr<g2o::SparseOptimizer> TebOptimalPlanner::initOptimizer()
{
    // Call register_g2o_types once, even for multiple TebOptimalPlanner instances (thread-safe)
    static boost::once_flag flag = BOOST_ONCE_INIT;
    boost::call_once(&registerG2OTypes, flag); // boost::call_once用于保证某个函数只执行一次, 不会被多个线程同时执行

    // allocating the optimizer
    boost::shared_ptr<g2o::SparseOptimizer> optimizer = boost::make_shared<g2o::SparseOptimizer>();
    TEBLinearSolver *linearSolver = new TEBLinearSolver(); // see typedef in optimization.h
    linearSolver->setBlockOrdering(true);
    TEBBlockSolver *blockSolver = new TEBBlockSolver(linearSolver);
    g2o::OptimizationAlgorithmLevenberg *solver = new g2o::OptimizationAlgorithmLevenberg(blockSolver);

    optimizer->setAlgorithm(solver);

    optimizer->initMultiThreading(); // required for >Eigen 3.1

    return optimizer;
}

bool TebOptimalPlanner::optimizeTEB(int iterations_innerloop, int iterations_outerloop, bool compute_cost_afterwards,
                                    double obst_cost_scale, double viapoint_cost_scale, bool alternative_time_cost)
{
    if (cfg_->optim.optimization_activate == false)
        return false;

    bool success = false;
    optimized_ = false;

    double weight_multiplier = 1.0;

    // TODO(roesmann): we introduced the non-fast mode with the support of dynamic obstacles
    //                (which leads to better results in terms of x-y-t homotopy planning).
    //                 however, we have not tested this mode intensively yet, so we keep
    //                 the legacy fast mode as default until we finish our tests.
    bool fast_mode = !cfg_->obstacles.include_dynamic_obstacles; // include_dynamic_obstacles = true

    for (int i = 0; i < iterations_outerloop; ++i)
    {
        if (cfg_->trajectory.teb_autosize) // 外环通过autoResize调整配置序列和delta_t
        {
            // dt_ref = 0.3, dt_hysteresis = 0.1, min_samples = 3, max_samples = 500
            teb_.autoResize(cfg_->trajectory.dt_ref, cfg_->trajectory.dt_hysteresis, cfg_->trajectory.min_samples, cfg_->trajectory.max_samples, fast_mode);
        }

        success = buildGraph(weight_multiplier);
        if (!success)
        {
            clearGraph();
            return false;
        }
        success = optimizeGraph(iterations_innerloop, false);
        if (!success)
        {
            clearGraph();
            return false;
        }
        optimized_ = true;

        if (compute_cost_afterwards && i == iterations_outerloop - 1) // compute cost vec only in the last iteration
            computeCurrentCost(obst_cost_scale, viapoint_cost_scale, alternative_time_cost);

        clearGraph();

        // Increasing weights iteratively instead of setting a huge value a-priori leads to better numerical conditions of the underlying optimization problem.
        weight_multiplier *= cfg_->optim.weight_adapt_factor; // weight_adapt_factor = 2
    }

    return true;
}

void TebOptimalPlanner::setVelocityStart(const geometry_msgs::Twist &vel_start)
{
    vel_start_.first = true;
    vel_start_.second.linear.x = vel_start.linear.x;
    vel_start_.second.linear.y = vel_start.linear.y;
    vel_start_.second.angular.z = vel_start.angular.z;
}

void TebOptimalPlanner::setVelocityGoal(const geometry_msgs::Twist &vel_goal)
{
    vel_goal_.first = true;
    vel_goal_.second = vel_goal;
}

bool TebOptimalPlanner::plan(const std::vector<geometry_msgs::PoseStamped> &initial_plan, const geometry_msgs::Twist *start_vel, bool free_goal_vel)
{
    ROS_ASSERT_MSG(initialized_, "Call initialize() first.");
    if (!teb_.isInit())
    {
        // init trajectory
        teb_.initTrajectoryToGoal(initial_plan, cfg_->robot.max_vel_x, cfg_->trajectory.global_plan_overwrite_orientation, cfg_->trajectory.min_samples, cfg_->trajectory.allow_init_with_backwards_motion);
    }
    else // warm start
    {
        PoseSE2 start_(initial_plan.front().pose);
        PoseSE2 goal_(initial_plan.back().pose);

        // force_reinit_new_goal_dist = 1 m
        if (teb_.sizePoses() > 0 && (goal_.position() - teb_.BackPose().position()).norm() < cfg_->trajectory.force_reinit_new_goal_dist && fabs(g2o::normalize_theta(goal_.theta() - teb_.BackPose().theta())) < cfg_->trajectory.force_reinit_new_goal_angular) // actual warm start!
            teb_.updateAndPruneTEB(start_, goal_, cfg_->trajectory.min_samples); // 剪枝掉已经经历过的TEB
        else // 这里goal_是local goal, 处于不断向前刷新中, 当local goal和当前teb末端距离较大时, reinit
        {
            ROS_DEBUG("New goal: distance to existing goal is higher than the specified threshold. Reinitalizing trajectories.");
            teb_.clearTimedElasticBand(); // 清空pose_vec_和timediff_vec_
            teb_.initTrajectoryToGoal(initial_plan, cfg_->robot.max_vel_x, true, cfg_->trajectory.min_samples, cfg_->trajectory.allow_init_with_backwards_motion);
        }
    }
    if (start_vel)
        setVelocityStart(*start_vel);
    if (free_goal_vel)
        setVelocityGoalFree();
    else
        vel_goal_.first = true; // we just reactivate and use the previously set velocity (should be zero if nothing was modified)

    // now optimize
    return optimizeTEB(cfg_->optim.no_inner_iterations, cfg_->optim.no_outer_iterations);
}

bool TebOptimalPlanner::plan(const tf::Pose &start, const tf::Pose &goal, const geometry_msgs::Twist *start_vel, bool free_goal_vel)
{
    PoseSE2 start_(start);
    PoseSE2 goal_(goal);
    return plan(start_, goal_, start_vel);
}

bool TebOptimalPlanner::plan(const PoseSE2 &start, const PoseSE2 &goal, const geometry_msgs::Twist *start_vel, bool free_goal_vel)
{
    ROS_ASSERT_MSG(initialized_, "Call initialize() first.");
    if (!teb_.isInit())
    {
        // init trajectory
        teb_.initTrajectoryToGoal(start, goal, 0, cfg_->robot.max_vel_x, cfg_->trajectory.min_samples, cfg_->trajectory.allow_init_with_backwards_motion); // 0 intermediate samples, but dt=1 -> autoResize will add more samples before calling first optimization
    }
    else // warm start
    {
        // trajectory.force_reinit_new_goal_dist = 1 m, trajectory.force_reinit_new_goal_angular = 0.5 * pi rad
        if (teb_.sizePoses() > 0 && (goal.position() - teb_.BackPose().position()).norm() < cfg_->trajectory.force_reinit_new_goal_dist && fabs(g2o::normalize_theta(goal.theta() - teb_.BackPose().theta())) < cfg_->trajectory.force_reinit_new_goal_angular) // actual warm start!
            teb_.updateAndPruneTEB(start, goal, cfg_->trajectory.min_samples); // 正常机器人运行时, 要剪枝掉已经经历的配置
        else // goal too far away -> reinit, 可能是更换goal的时候
        {
            ROS_DEBUG("New goal: distance to existing goal is higher than the specified threshold. Reinitalizing trajectories.");
            teb_.clearTimedElasticBand(); // 清空pose_vec_和timediff_vec_
            teb_.initTrajectoryToGoal(start, goal, 0, cfg_->robot.max_vel_x, cfg_->trajectory.min_samples, cfg_->trajectory.allow_init_with_backwards_motion);
        }
    }
    if (start_vel)
        setVelocityStart(*start_vel);
    if (free_goal_vel)
        setVelocityGoalFree(); // 不限制终点速度
    else
        vel_goal_.first = true; // we just reactivate and use the previously set velocity (should be zero if nothing was modified)

    // now optimize
    return optimizeTEB(cfg_->optim.no_inner_iterations, cfg_->optim.no_outer_iterations); // inner -> 5, outer -> 4
}

bool TebOptimalPlanner::buildGraph(double weight_multiplier)
{
    if (!optimizer_->edges().empty() || !optimizer_->vertices().empty())
    {
        ROS_WARN("Cannot build graph, because it is not empty. Call graphClear()!");
        return false;
    }

    // add TEB vertices
    AddTEBVertices(); // 添加PoseVertex和TimeDiffVertex

    // add Edges (local cost functions)
    if (cfg_->obstacles.legacy_obstacle_association) // legacy_obstacle_association : false
        AddEdgesObstaclesLegacy(weight_multiplier); // old association strategy : for each obstacle, find the nearest TEB pose
    else
        AddEdgesObstacles(weight_multiplier); // new association strategy : for each teb pose, find only "relevant" obstacles

    if (cfg_->obstacles.include_dynamic_obstacles)
        AddEdgesDynamicObstacles();

    AddEdgesViaPoints();

    AddEdgesVelocity();

    AddEdgesAcceleration();

    AddEdgesTimeOptimal();

    AddEdgesShortestPath();

    if (cfg_->robot.min_turning_radius == 0 || cfg_->optim.weight_kinematics_turning_radius == 0)
        AddEdgesKinematicsDiffDrive(); // we have a differential drive robot
    else
        AddEdgesKinematicsCarlike(); // we have a carlike robot since the turning radius is bounded from below.

    AddEdgesPreferRotDir();

    return true;
}

bool TebOptimalPlanner::optimizeGraph(int no_iterations, bool clear_after)
{
    if (cfg_->robot.max_vel_x < 0.01)
    {
        ROS_WARN("optimizeGraph(): Robot Max Velocity is smaller than 0.01m/s. Optimizing aborted...");
        if (clear_after)
            clearGraph();
        return false;
    }

    if (!teb_.isInit() || teb_.sizePoses() < cfg_->trajectory.min_samples)
    {
        ROS_WARN("optimizeGraph(): TEB is empty or has too less elements. Skipping optimization.");
        if (clear_after)
            clearGraph();
        return false;
    }

    optimizer_->setVerbose(cfg_->optim.optimization_verbose); // Print verbose information or not
    optimizer_->initializeOptimization();

    int iter = optimizer_->optimize(no_iterations);

    // Save Hessian for visualization
    //  g2o::OptimizationAlgorithmLevenberg* lm = dynamic_cast<g2o::OptimizationAlgorithmLevenberg*> (optimizer_->solver());
    //  lm->solver()->saveHessian("~/MasterThesis/Matlab/Hessian.txt");

    if (!iter)
    {
        ROS_ERROR("optimizeGraph(): Optimization failed! iter=%i", iter);
        return false;
    }

    if (clear_after)
        clearGraph();

    return true;
}

void TebOptimalPlanner::clearGraph()
{
    // clear optimizer states
    if (optimizer_)
    {
        // optimizer.edges().clear(); // optimizer.clear deletes edges!!! Therefore do not run optimizer.edges().clear()
        optimizer_->vertices().clear(); // neccessary, because optimizer->clear deletes pointer-targets (therefore it deletes TEB states!)
        optimizer_->clear();
    }
}

void TebOptimalPlanner::AddTEBVertices()
{
    // add vertices to graph
    ROS_DEBUG_COND(cfg_->optim.optimization_verbose, "Adding TEB vertices ...");
    unsigned int id_counter = 0; // used for vertices ids
    for (int i = 0; i < teb_.sizePoses(); ++i)
    {
        teb_.PoseVertex(i)->setId(id_counter++);
        optimizer_->addVertex(teb_.PoseVertex(i));
        if (teb_.sizeTimeDiffs() != 0 && i < teb_.sizeTimeDiffs())
        {
            teb_.TimeDiffVertex(i)->setId(id_counter++);
            optimizer_->addVertex(teb_.TimeDiffVertex(i));
        }
    }
}

void TebOptimalPlanner::AddEdgesObstacles(double weight_multiplier)
{
    if (cfg_->optim.weight_obstacle == 0 || weight_multiplier == 0 || obstacles_ == nullptr)
        return; // if weight equals zero skip adding edges!

    bool inflated = cfg_->obstacles.inflation_dist > cfg_->obstacles.min_obstacle_dist;

    Eigen::Matrix<double, 1, 1> information;
    information.fill(cfg_->optim.weight_obstacle * weight_multiplier);

    Eigen::Matrix<double, 2, 2> information_inflated;
    information_inflated(0, 0) = cfg_->optim.weight_obstacle * weight_multiplier; // weight_obstacle = 50
    information_inflated(1, 1) = cfg_->optim.weight_inflation; // weight_inflation = 0.1
    information_inflated(0, 1) = information_inflated(1, 0) = 0;

    // iterate all teb points (skip first and last)
    for (int i = 1; i < teb_.sizePoses() - 1; ++i)
    {
        double left_min_dist = std::numeric_limits<double>::max();
        double right_min_dist = std::numeric_limits<double>::max();
        Obstacle *left_obstacle = nullptr;
        Obstacle *right_obstacle = nullptr;

        std::vector<Obstacle *> relevant_obstacles;

        const Eigen::Vector2d pose_orient = teb_.Pose(i).orientationUnitVec();

        // iterate obstacles
        for (const ObstaclePtr &obst : *obstacles_)
        {
            // we handle dynamic obstacles differently below
            if (cfg_->obstacles.include_dynamic_obstacles && obst->isDynamic())
                continue;

            // calculate distance to robot model
            double dist = robot_model_->calculateDistance(teb_.Pose(i), obst.get());

            // force considering obstacle if really close to the current pose
            if (dist < cfg_->obstacles.min_obstacle_dist * cfg_->obstacles.obstacle_association_force_inclusion_factor) // obstacle_association_force_inclusion_factor = 1.5
            {
                relevant_obstacles.push_back(obst.get());
                continue;
            }
            // cut-off distance
            if (dist > cfg_->obstacles.min_obstacle_dist * cfg_->obstacles.obstacle_association_cutoff_factor) // obstacle_association_cutoff_factor = 5
                continue;

            // determine side (left or right) and assign obstacle if closer than the previous one
            if (cross2d(pose_orient, obst->getCentroid()) > 0) // left
            {
                if (dist < left_min_dist)
                {
                    left_min_dist = dist; // min distance of obstacle in left
                    left_obstacle = obst.get();
                }
            }
            else // right
            {
                if (dist < right_min_dist)
                {
                    right_min_dist = dist; // min distance of obstacle in right
                    right_obstacle = obst.get();
                }
            }
        }

        // create obstacle edges, 包括三部分 : 左面最近的障碍, 右面最近的障碍, relevant_obstacles中包含的危险区域的障碍
        if (left_obstacle)
        {
            if (inflated)
            {
                EdgeInflatedObstacle *dist_bandpt_obst = new EdgeInflatedObstacle;
                dist_bandpt_obst->setVertex(0, teb_.PoseVertex(i));
                dist_bandpt_obst->setInformation(information_inflated);
                dist_bandpt_obst->setParameters(*cfg_, robot_model_.get(), left_obstacle);
                optimizer_->addEdge(dist_bandpt_obst);
            }
            else
            {
                EdgeObstacle *dist_bandpt_obst = new EdgeObstacle;
                dist_bandpt_obst->setVertex(0, teb_.PoseVertex(i));
                dist_bandpt_obst->setInformation(information);
                dist_bandpt_obst->setParameters(*cfg_, robot_model_.get(), left_obstacle);
                optimizer_->addEdge(dist_bandpt_obst);
            }
        }

        if (right_obstacle)
        {
            if (inflated)
            {
                EdgeInflatedObstacle *dist_bandpt_obst = new EdgeInflatedObstacle;
                dist_bandpt_obst->setVertex(0, teb_.PoseVertex(i));
                dist_bandpt_obst->setInformation(information_inflated);
                dist_bandpt_obst->setParameters(*cfg_, robot_model_.get(), right_obstacle);
                optimizer_->addEdge(dist_bandpt_obst);
            }
            else
            {
                EdgeObstacle *dist_bandpt_obst = new EdgeObstacle;
                dist_bandpt_obst->setVertex(0, teb_.PoseVertex(i));
                dist_bandpt_obst->setInformation(information);
                dist_bandpt_obst->setParameters(*cfg_, robot_model_.get(), right_obstacle);
                optimizer_->addEdge(dist_bandpt_obst);
            }
        }

        for (const Obstacle *obst : relevant_obstacles)
        {
            if (inflated)
            {
                EdgeInflatedObstacle *dist_bandpt_obst = new EdgeInflatedObstacle;
                dist_bandpt_obst->setVertex(0, teb_.PoseVertex(i));
                dist_bandpt_obst->setInformation(information_inflated);
                dist_bandpt_obst->setParameters(*cfg_, robot_model_.get(), obst);
                optimizer_->addEdge(dist_bandpt_obst);
            }
            else
            {
                EdgeObstacle *dist_bandpt_obst = new EdgeObstacle;
                dist_bandpt_obst->setVertex(0, teb_.PoseVertex(i));
                dist_bandpt_obst->setInformation(information);
                dist_bandpt_obst->setParameters(*cfg_, robot_model_.get(), obst);
                optimizer_->addEdge(dist_bandpt_obst);
            }
        }
    }
}

void TebOptimalPlanner::AddEdgesObstaclesLegacy(double weight_multiplier)
{
    if (cfg_->optim.weight_obstacle == 0 || weight_multiplier == 0 || obstacles_ == nullptr)
        return; // if weight equals zero skip adding edges!

    Eigen::Matrix<double, 1, 1> information;
    information.fill(cfg_->optim.weight_obstacle * weight_multiplier); // weight_obstacle = 50

    Eigen::Matrix<double, 2, 2> information_inflated;
    information_inflated(0, 0) = cfg_->optim.weight_obstacle * weight_multiplier;
    information_inflated(1, 1) = cfg_->optim.weight_inflation; // weight_inflation = 0.1, weight for the inflation penalty
    information_inflated(0, 1) = information_inflated(1, 0) = 0;

    // inflation_dist表示优化障碍物时的缓冲距离, 即在min_obstacle_dist ~ inflation_dist的范围内惩罚值不为0
    bool inflated = cfg_->obstacles.inflation_dist > cfg_->obstacles.min_obstacle_dist; // 0.6 > 0.5

    for (ObstContainer::const_iterator obst = obstacles_->begin(); obst != obstacles_->end(); ++obst)
    {
        if (cfg_->obstacles.include_dynamic_obstacles && (*obst)->isDynamic()) // we handle dynamic obstacles differently below
            continue;

        int index;

        // obstacle_poses_affected表示距离障碍物最近的pose及其邻居的个数, obstacle_poses_affected = 25
        if (cfg_->obstacles.obstacle_poses_affected >= teb_.sizePoses())
            index = teb_.sizePoses() / 2;
        else
            index = teb_.findClosestTrajectoryPose(*(obst->get())); // obst为迭代器, obst->get()为障碍物指针

        // check if obstacle is outside index-range between start and goal
        if ((index <= 1) || (index > teb_.sizePoses() - 2)) // start and goal are fixed and findNearestBandpoint finds first or last conf if intersection point is outside the range
            continue;

        if (inflated) // Inflated Obstacle
        {
            EdgeInflatedObstacle *dist_bandpt_obst = new EdgeInflatedObstacle;
            dist_bandpt_obst->setVertex(0, teb_.PoseVertex(index)); // 每个EdgeInflatedObstacle只包含一个VertexPose
            dist_bandpt_obst->setInformation(information_inflated);
            dist_bandpt_obst->setParameters(*cfg_, robot_model_.get(), obst->get());
            optimizer_->addEdge(dist_bandpt_obst);
        }
        else //  Original Obstacle
        {
            EdgeObstacle *dist_bandpt_obst = new EdgeObstacle;
            dist_bandpt_obst->setVertex(0, teb_.PoseVertex(index));
            dist_bandpt_obst->setInformation(information);
            dist_bandpt_obst->setParameters(*cfg_, robot_model_.get(), obst->get()); // get返回原始指针
            optimizer_->addEdge(dist_bandpt_obst);
        }

        for (int neighbourIdx = 0; neighbourIdx < floor(cfg_->obstacles.obstacle_poses_affected / 2); neighbourIdx++)
        {
            if (index + neighbourIdx < teb_.sizePoses()) // 当前index右面的配置
            {
                if (inflated)
                {
                    EdgeInflatedObstacle *dist_bandpt_obst_n_r = new EdgeInflatedObstacle;
                    dist_bandpt_obst_n_r->setVertex(0, teb_.PoseVertex(index + neighbourIdx));
                    dist_bandpt_obst_n_r->setInformation(information_inflated);
                    dist_bandpt_obst_n_r->setParameters(*cfg_, robot_model_.get(), obst->get());
                    optimizer_->addEdge(dist_bandpt_obst_n_r);
                }
                else
                {
                    EdgeObstacle *dist_bandpt_obst_n_r = new EdgeObstacle;
                    dist_bandpt_obst_n_r->setVertex(0, teb_.PoseVertex(index + neighbourIdx));
                    dist_bandpt_obst_n_r->setInformation(information);
                    dist_bandpt_obst_n_r->setParameters(*cfg_, robot_model_.get(), obst->get());
                    optimizer_->addEdge(dist_bandpt_obst_n_r);
                }
            }
            if (index - neighbourIdx >= 0) // 当前index左面的配置
            {
                if (inflated)
                {
                    EdgeInflatedObstacle *dist_bandpt_obst_n_l = new EdgeInflatedObstacle;
                    dist_bandpt_obst_n_l->setVertex(0, teb_.PoseVertex(index - neighbourIdx));
                    dist_bandpt_obst_n_l->setInformation(information_inflated);
                    dist_bandpt_obst_n_l->setParameters(*cfg_, robot_model_.get(), obst->get());
                    optimizer_->addEdge(dist_bandpt_obst_n_l);
                }
                else
                {
                    EdgeObstacle *dist_bandpt_obst_n_l = new EdgeObstacle;
                    dist_bandpt_obst_n_l->setVertex(0, teb_.PoseVertex(index - neighbourIdx));
                    dist_bandpt_obst_n_l->setInformation(information);
                    dist_bandpt_obst_n_l->setParameters(*cfg_, robot_model_.get(), obst->get());
                    optimizer_->addEdge(dist_bandpt_obst_n_l);
                }
            }
        }
    }
}

void TebOptimalPlanner::AddEdgesDynamicObstacles(double weight_multiplier)
{
    if (cfg_->optim.weight_obstacle == 0 || weight_multiplier == 0 || obstacles_ == NULL)
        return; // if weight equals zero skip adding edges!

    Eigen::Matrix<double, 2, 2> information;
    information(0, 0) = cfg_->optim.weight_dynamic_obstacle * weight_multiplier;
    information(1, 1) = cfg_->optim.weight_dynamic_obstacle_inflation;
    information(0, 1) = information(1, 0) = 0;

    for (ObstContainer::const_iterator obst = obstacles_->begin(); obst != obstacles_->end(); ++obst)
    {
        if (!(*obst)->isDynamic())
            continue;

        // Skip first and last pose, as they are fixed
        double time = teb_.TimeDiff(0);
        for (int i = 1; i < teb_.sizePoses() - 1; ++i) // 动态障碍可能影响轨迹中的每个点, 所以每个Pose都需要考虑
        {
            EdgeDynamicObstacle *dynobst_edge = new EdgeDynamicObstacle(time);
            dynobst_edge->setVertex(0, teb_.PoseVertex(i));
            dynobst_edge->setInformation(information);
            dynobst_edge->setParameters(*cfg_, robot_model_.get(), obst->get());
            optimizer_->addEdge(dynobst_edge);
            time += teb_.TimeDiff(i); // we do not need to check the time diff bounds, since we iterate to "< sizePoses()-1".
        }
    }
}

void TebOptimalPlanner::AddEdgesViaPoints()
{
    if (cfg_->optim.weight_viapoint == 0 || via_points_ == NULL || via_points_->empty())
        return; // if weight equals zero skip adding edges!

    int start_pose_idx = 0;

    int n = teb_.sizePoses();
    if (n < 3) // we do not have any degrees of freedom for reaching via-points
        return;

    for (ViaPointContainer::const_iterator vp_it = via_points_->begin(); vp_it != via_points_->end(); ++vp_it)
    {

        int index = teb_.findClosestTrajectoryPose(*vp_it, NULL, start_pose_idx);
        if (cfg_->trajectory.via_points_ordered)
            start_pose_idx = index + 2; // skip a point to have a DOF inbetween for further via-points

        // check if point conicides with goal or is located behind it
        if (index > n - 2)
            index = n - 2; // set to a pose before the goal, since we can move it away!
        // check if point coincides with start or is located before it
        if (index < 1)
        {
            if (cfg_->trajectory.via_points_ordered)
            {
                index = 1; // try to connect the via point with the second (and non-fixed) pose. It is likely that autoresize adds new poses inbetween later.
            }
            else
            {
                ROS_DEBUG("TebOptimalPlanner::AddEdgesViaPoints(): skipping a via-point that is close or behind the current robot pose.");
                continue; // skip via points really close or behind the current robot pose
            }
        }
        Eigen::Matrix<double, 1, 1> information;
        information.fill(cfg_->optim.weight_viapoint);

        EdgeViaPoint *edge_viapoint = new EdgeViaPoint;
        edge_viapoint->setVertex(0, teb_.PoseVertex(index));
        edge_viapoint->setInformation(information);
        edge_viapoint->setParameters(*cfg_, &(*vp_it)); // vp_it为迭代器而不是普通地址
        optimizer_->addEdge(edge_viapoint);
    }
}

void TebOptimalPlanner::AddEdgesVelocity()
{
    if (cfg_->robot.max_vel_y == 0) // non-holonomic robot
    {
        if (cfg_->optim.weight_max_vel_x == 0 && cfg_->optim.weight_max_vel_theta == 0)
            return; // if weight equals zero skip adding edges!

        int n = teb_.sizePoses();
        Eigen::Matrix<double, 2, 2> information;
        information(0, 0) = cfg_->optim.weight_max_vel_x;
        information(1, 1) = cfg_->optim.weight_max_vel_theta;
        information(0, 1) = 0.0;
        information(1, 0) = 0.0;

        for (int i = 0; i < n - 1; ++i)
        {
            EdgeVelocity *velocity_edge = new EdgeVelocity;
            velocity_edge->setVertex(0, teb_.PoseVertex(i));
            velocity_edge->setVertex(1, teb_.PoseVertex(i + 1));
            velocity_edge->setVertex(2, teb_.TimeDiffVertex(i));
            velocity_edge->setInformation(information);
            velocity_edge->setTebConfig(*cfg_);
            optimizer_->addEdge(velocity_edge);
        }
    }
    else // holonomic-robot
    {
        if (cfg_->optim.weight_max_vel_x == 0 && cfg_->optim.weight_max_vel_y == 0 && cfg_->optim.weight_max_vel_theta == 0)
            return; // if weight equals zero skip adding edges!

        int n = teb_.sizePoses();
        Eigen::Matrix<double, 3, 3> information;
        information.fill(0);
        information(0, 0) = cfg_->optim.weight_max_vel_x;
        information(1, 1) = cfg_->optim.weight_max_vel_y;
        information(2, 2) = cfg_->optim.weight_max_vel_theta;

        for (int i = 0; i < n - 1; ++i)
        {
            EdgeVelocityHolonomic *velocity_edge = new EdgeVelocityHolonomic;
            velocity_edge->setVertex(0, teb_.PoseVertex(i));
            velocity_edge->setVertex(1, teb_.PoseVertex(i + 1));
            velocity_edge->setVertex(2, teb_.TimeDiffVertex(i));
            velocity_edge->setInformation(information);
            velocity_edge->setTebConfig(*cfg_);
            optimizer_->addEdge(velocity_edge);
        }
    }
}

void TebOptimalPlanner::AddEdgesAcceleration()
{
    if (cfg_->optim.weight_acc_lim_x == 0 && cfg_->optim.weight_acc_lim_theta == 0)
        return; // if weight equals zero skip adding edges!

    int n = teb_.sizePoses();

    if (cfg_->robot.max_vel_y == 0 || cfg_->robot.acc_lim_y == 0) // non-holonomic robot
    {
        Eigen::Matrix<double, 2, 2> information;
        information.fill(0);
        information(0, 0) = cfg_->optim.weight_acc_lim_x;
        information(1, 1) = cfg_->optim.weight_acc_lim_theta;

        // check if an initial velocity should be taken into account
        if (vel_start_.first)
        {
            EdgeAccelerationStart *acceleration_edge = new EdgeAccelerationStart;
            acceleration_edge->setVertex(0, teb_.PoseVertex(0));
            acceleration_edge->setVertex(1, teb_.PoseVertex(1));
            acceleration_edge->setVertex(2, teb_.TimeDiffVertex(0));
            acceleration_edge->setInitialVelocity(vel_start_.second);
            acceleration_edge->setInformation(information);
            acceleration_edge->setTebConfig(*cfg_);
            optimizer_->addEdge(acceleration_edge);
        }

        // now add the usual acceleration edge for each tuple of three teb poses
        for (int i = 0; i < n - 2; ++i)
        {
            EdgeAcceleration *acceleration_edge = new EdgeAcceleration;
            acceleration_edge->setVertex(0, teb_.PoseVertex(i));
            acceleration_edge->setVertex(1, teb_.PoseVertex(i + 1));
            acceleration_edge->setVertex(2, teb_.PoseVertex(i + 2));
            acceleration_edge->setVertex(3, teb_.TimeDiffVertex(i));
            acceleration_edge->setVertex(4, teb_.TimeDiffVertex(i + 1));
            acceleration_edge->setInformation(information);
            acceleration_edge->setTebConfig(*cfg_);
            optimizer_->addEdge(acceleration_edge);
        }

        // check if a goal velocity should be taken into account
        if (vel_goal_.first)
        {
            EdgeAccelerationGoal *acceleration_edge = new EdgeAccelerationGoal;
            acceleration_edge->setVertex(0, teb_.PoseVertex(n - 2));
            acceleration_edge->setVertex(1, teb_.PoseVertex(n - 1));
            acceleration_edge->setVertex(2, teb_.TimeDiffVertex(teb_.sizeTimeDiffs() - 1));
            acceleration_edge->setGoalVelocity(vel_goal_.second);
            acceleration_edge->setInformation(information);
            acceleration_edge->setTebConfig(*cfg_);
            optimizer_->addEdge(acceleration_edge);
        }
    }
    else // holonomic robot
    {
        Eigen::Matrix<double, 3, 3> information;
        information.fill(0);
        information(0, 0) = cfg_->optim.weight_acc_lim_x;
        information(1, 1) = cfg_->optim.weight_acc_lim_y;
        information(2, 2) = cfg_->optim.weight_acc_lim_theta;

        // check if an initial velocity should be taken into account
        if (vel_start_.first)
        {
            EdgeAccelerationHolonomicStart *acceleration_edge = new EdgeAccelerationHolonomicStart;
            acceleration_edge->setVertex(0, teb_.PoseVertex(0));
            acceleration_edge->setVertex(1, teb_.PoseVertex(1));
            acceleration_edge->setVertex(2, teb_.TimeDiffVertex(0));
            acceleration_edge->setInitialVelocity(vel_start_.second);
            acceleration_edge->setInformation(information);
            acceleration_edge->setTebConfig(*cfg_);
            optimizer_->addEdge(acceleration_edge);
        }

        // now add the usual acceleration edge for each tuple of three teb poses
        for (int i = 0; i < n - 2; ++i)
        {
            EdgeAccelerationHolonomic *acceleration_edge = new EdgeAccelerationHolonomic;
            acceleration_edge->setVertex(0, teb_.PoseVertex(i));
            acceleration_edge->setVertex(1, teb_.PoseVertex(i + 1));
            acceleration_edge->setVertex(2, teb_.PoseVertex(i + 2));
            acceleration_edge->setVertex(3, teb_.TimeDiffVertex(i));
            acceleration_edge->setVertex(4, teb_.TimeDiffVertex(i + 1));
            acceleration_edge->setInformation(information);
            acceleration_edge->setTebConfig(*cfg_);
            optimizer_->addEdge(acceleration_edge);
        }

        // check if a goal velocity should be taken into account
        if (vel_goal_.first)
        {
            EdgeAccelerationHolonomicGoal *acceleration_edge = new EdgeAccelerationHolonomicGoal;
            acceleration_edge->setVertex(0, teb_.PoseVertex(n - 2));
            acceleration_edge->setVertex(1, teb_.PoseVertex(n - 1));
            acceleration_edge->setVertex(2, teb_.TimeDiffVertex(teb_.sizeTimeDiffs() - 1));
            acceleration_edge->setGoalVelocity(vel_goal_.second);
            acceleration_edge->setInformation(information);
            acceleration_edge->setTebConfig(*cfg_);
            optimizer_->addEdge(acceleration_edge);
        }
    }
}

void TebOptimalPlanner::AddEdgesTimeOptimal()
{
    if (cfg_->optim.weight_optimaltime == 0)
        return; // if weight equals zero skip adding edges!

    Eigen::Matrix<double, 1, 1> information;
    information.fill(cfg_->optim.weight_optimaltime);

    for (int i = 0; i < teb_.sizeTimeDiffs(); ++i)
    {
        EdgeTimeOptimal *timeoptimal_edge = new EdgeTimeOptimal;
        timeoptimal_edge->setVertex(0, teb_.TimeDiffVertex(i));
        timeoptimal_edge->setInformation(information);
        timeoptimal_edge->setTebConfig(*cfg_);
        optimizer_->addEdge(timeoptimal_edge);
    }
}

void TebOptimalPlanner::AddEdgesShortestPath()
{
    if (cfg_->optim.weight_shortest_path == 0)
        return; // if weight equals zero skip adding edges!

    Eigen::Matrix<double, 1, 1> information;
    information.fill(cfg_->optim.weight_shortest_path);

    for (int i = 0; i < teb_.sizePoses() - 1; ++i)
    {
        EdgeShortestPath *shortest_path_edge = new EdgeShortestPath;
        shortest_path_edge->setVertex(0, teb_.PoseVertex(i));
        shortest_path_edge->setVertex(1, teb_.PoseVertex(i + 1));
        shortest_path_edge->setInformation(information);
        shortest_path_edge->setTebConfig(*cfg_);
        optimizer_->addEdge(shortest_path_edge);
    }
}

void TebOptimalPlanner::AddEdgesKinematicsDiffDrive()
{
    if (cfg_->optim.weight_kinematics_nh == 0 && cfg_->optim.weight_kinematics_forward_drive == 0)
        return; // if weight equals zero skip adding edges!

    // create edge for satisfiying kinematic constraints
    Eigen::Matrix<double, 2, 2> information_kinematics;
    information_kinematics.fill(0.0);
    information_kinematics(0, 0) = cfg_->optim.weight_kinematics_nh;
    information_kinematics(1, 1) = cfg_->optim.weight_kinematics_forward_drive;

    for (int i = 0; i < teb_.sizePoses() - 1; i++) // ignore twiced start only
    {
        EdgeKinematicsDiffDrive *kinematics_edge = new EdgeKinematicsDiffDrive;
        kinematics_edge->setVertex(0, teb_.PoseVertex(i));
        kinematics_edge->setVertex(1, teb_.PoseVertex(i + 1));
        kinematics_edge->setInformation(information_kinematics);
        kinematics_edge->setTebConfig(*cfg_);
        optimizer_->addEdge(kinematics_edge);
    }
}

void TebOptimalPlanner::AddEdgesKinematicsCarlike()
{
    if (cfg_->optim.weight_kinematics_nh == 0 && cfg_->optim.weight_kinematics_turning_radius == 0)
        return; // if weight equals zero skip adding edges!

    // create edge for satisfiying kinematic constraints
    Eigen::Matrix<double, 2, 2> information_kinematics;
    information_kinematics.fill(0.0);
    information_kinematics(0, 0) = cfg_->optim.weight_kinematics_nh;
    information_kinematics(1, 1) = cfg_->optim.weight_kinematics_turning_radius;

    for (int i = 0; i < teb_.sizePoses() - 1; i++) // ignore twiced start only
    {
        EdgeKinematicsCarlike *kinematics_edge = new EdgeKinematicsCarlike;
        kinematics_edge->setVertex(0, teb_.PoseVertex(i));
        kinematics_edge->setVertex(1, teb_.PoseVertex(i + 1));
        kinematics_edge->setInformation(information_kinematics);
        kinematics_edge->setTebConfig(*cfg_);
        optimizer_->addEdge(kinematics_edge);
    }
}

void TebOptimalPlanner::AddEdgesPreferRotDir()
{
    //TODO(roesmann): Note, these edges can result in odd predictions, in particular
    //                we can observe a substantional mismatch between open- and closed-loop planning
    //                leading to a poor control performance.
    //                At the moment, we keep these functionality for oscillation recovery:
    //                Activating the edge for a short time period might not be crucial and
    //                could move the robot to a new oscillation-free state.
    //                This needs to be analyzed in more detail!
    if (prefer_rotdir_ == RotType::none || cfg_->optim.weight_prefer_rotdir == 0)
        return; // if weight equals zero skip adding edges!

    if (prefer_rotdir_ != RotType::right && prefer_rotdir_ != RotType::left)
    {
        ROS_WARN("TebOptimalPlanner::AddEdgesPreferRotDir(): unsupported RotType selected. Skipping edge creation.");
        return;
    }

    // create edge for satisfiying kinematic constraints
    Eigen::Matrix<double, 1, 1> information_rotdir;
    information_rotdir.fill(cfg_->optim.weight_prefer_rotdir);

    for (int i = 0; i < teb_.sizePoses() - 1 && i < 3; ++i) // currently: apply to first 3 rotations
    {
        EdgePreferRotDir *rotdir_edge = new EdgePreferRotDir;
        rotdir_edge->setVertex(0, teb_.PoseVertex(i));
        rotdir_edge->setVertex(1, teb_.PoseVertex(i + 1));
        rotdir_edge->setInformation(information_rotdir);

        if (prefer_rotdir_ == RotType::left)
            rotdir_edge->preferLeft();
        else if (prefer_rotdir_ == RotType::right)
            rotdir_edge->preferRight();

        optimizer_->addEdge(rotdir_edge);
    }
}

void TebOptimalPlanner::computeCurrentCost(double obst_cost_scale, double viapoint_cost_scale, bool alternative_time_cost)
{
    // check if graph is empty/exist  -> important if function is called between buildGraph and optimizeGraph/clearGraph
    bool graph_exist_flag(false);
    if (optimizer_->edges().empty() && optimizer_->vertices().empty())
    {
        // here the graph is build again, for time efficiency make sure to call this function
        // between buildGraph and Optimize (deleted), but it depends on the application
        buildGraph();
        optimizer_->initializeOptimization();
    }
    else
    {
        graph_exist_flag = true;
    }

    optimizer_->computeInitialGuess();

    cost_ = 0;

    if (alternative_time_cost)
    {
        cost_ += teb_.getSumOfAllTimeDiffs();
        // TEST we use SumOfAllTimeDiffs() here, because edge cost depends on number of samples, which is not always the same for similar TEBs,
        // since we are using an AutoResize Function with hysteresis.
    }

    // now we need pointers to all edges -> calculate error for each edge-type
    // since we aren't storing edge pointers, we need to check every edge
    for (std::vector<g2o::OptimizableGraph::Edge *>::const_iterator it = optimizer_->activeEdges().begin(); it != optimizer_->activeEdges().end(); it++)
    {
        double cur_cost = (*it)->chi2();

        if (dynamic_cast<EdgeObstacle *>(*it) != nullptr || dynamic_cast<EdgeInflatedObstacle *>(*it) != nullptr || dynamic_cast<EdgeDynamicObstacle *>(*it) != nullptr)
        {
            cur_cost *= obst_cost_scale;
        }
        else if (dynamic_cast<EdgeViaPoint *>(*it) != nullptr)
        {
            cur_cost *= viapoint_cost_scale;
        }
        else if (dynamic_cast<EdgeTimeOptimal *>(*it) != nullptr && alternative_time_cost)
        {
            continue; // skip these edges if alternative_time_cost is active
        }
        cost_ += cur_cost;
    }

    // delete temporary created graph
    if (!graph_exist_flag)
        clearGraph();
}

void TebOptimalPlanner::extractVelocity(const PoseSE2 &pose1, const PoseSE2 &pose2, double dt, double &vx, double &vy, double &omega) const
{
    if (dt == 0)
    {
        vx = 0;
        vy = 0;
        omega = 0;
        return;
    }

    Eigen::Vector2d deltaS = pose2.position() - pose1.position();

    if (cfg_->robot.max_vel_y == 0) // nonholonomic robot
    {
        Eigen::Vector2d conf1dir(cos(pose1.theta()), sin(pose1.theta()));
        // translational velocity
        double dir = deltaS.dot(conf1dir);
        vx = (double)g2o::sign(dir) * deltaS.norm() / dt; // 若轨迹方向和当前位姿方向相反, 则vx为负
        vy = 0;
    }
    else // holonomic robot
    {
        // transform pose 2 into the current robot frame (pose1)
        // for velocities only the rotation of the direction vector is necessary.
        // (map->pose1-frame: inverse 2d rotation matrix)
        double cos_theta1 = std::cos(pose1.theta());
        double sin_theta1 = std::sin(pose1.theta());
        double p1_dx = cos_theta1 * deltaS.x() + sin_theta1 * deltaS.y(); // 将world系下两个POSE组成的向量变换到robot系下
        double p1_dy = -sin_theta1 * deltaS.x() + cos_theta1 * deltaS.y(); // 因为设计到速度分解, 所以需要将向量变换到robot系下
        vx = p1_dx / dt;
        vy = p1_dy / dt;
    }

    // rotational velocity
    double orientdiff = g2o::normalize_theta(pose2.theta() - pose1.theta());
    omega = orientdiff / dt;
}

bool TebOptimalPlanner::getVelocityCommand(double &vx, double &vy, double &omega, int look_ahead_poses) const
{
    if (teb_.sizePoses() < 2)
    {
        ROS_ERROR("TebOptimalPlanner::getVelocityCommand(): The trajectory contains less than 2 poses. Make sure to init and optimize/plan the trajectory fist.");
        vx = 0;
        vy = 0;
        omega = 0;
        return false;
    }
    // default : look_ahead_poses = 1
    look_ahead_poses = std::max(1, std::min(look_ahead_poses, teb_.sizePoses() - 1));
    double dt = 0.0;
    for (int counter = 0; counter < look_ahead_poses; ++counter)
    {
        dt += teb_.TimeDiff(counter);
        // dt_ref = 0.3, Desired temporal resolution of the trajectory
        if (dt >= cfg_->trajectory.dt_ref * look_ahead_poses) // TODO: change to look-ahead time? Refine trajectory?
        {
            look_ahead_poses = counter + 1;
            break; // 轨迹在时间域太离散, 减少look_ahead_poses
        }
    }
    if (dt <= 0)
    {
        ROS_ERROR("TebOptimalPlanner::getVelocityCommand() - timediff<=0 is invalid!");
        vx = 0;
        vy = 0;
        omega = 0;
        return false;
    }

    // Get velocity from the first two configurations
    extractVelocity(teb_.Pose(0), teb_.Pose(look_ahead_poses), dt, vx, vy, omega);
    return true;
}

void TebOptimalPlanner::getVelocityProfile(std::vector<geometry_msgs::Twist> &velocity_profile) const
{
    int n = teb_.sizePoses();
    velocity_profile.resize(n + 1);

    // start velocity
    velocity_profile.front().linear.z = 0;
    velocity_profile.front().angular.x = velocity_profile.front().angular.y = 0;
    velocity_profile.front().linear.x = vel_start_.second.linear.x;
    velocity_profile.front().linear.y = vel_start_.second.linear.y;
    velocity_profile.front().angular.z = vel_start_.second.angular.z;

    for (int i = 1; i < n; ++i)
    {
        velocity_profile[i].linear.z = 0;
        velocity_profile[i].angular.x = velocity_profile[i].angular.y = 0;
        extractVelocity(teb_.Pose(i - 1), teb_.Pose(i), teb_.TimeDiff(i - 1), velocity_profile[i].linear.x, velocity_profile[i].linear.y, velocity_profile[i].angular.z);
    }

    // goal velocity
    velocity_profile.back().linear.z = 0;
    velocity_profile.back().angular.x = velocity_profile.back().angular.y = 0;
    velocity_profile.back().linear.x = vel_goal_.second.linear.x;
    velocity_profile.back().linear.y = vel_goal_.second.linear.y;
    velocity_profile.back().angular.z = vel_goal_.second.angular.z;
}

void TebOptimalPlanner::getFullTrajectory(std::vector<TrajectoryPointMsg> &trajectory) const
{
    int n = teb_.sizePoses();

    trajectory.resize(n);

    if (n == 0)
        return;

    double curr_time = 0;

    // start
    TrajectoryPointMsg &start = trajectory.front();
    teb_.Pose(0).toPoseMsg(start.pose);
    start.velocity.linear.z = 0;
    start.velocity.angular.x = start.velocity.angular.y = 0;
    start.velocity.linear.x = vel_start_.second.linear.x;
    start.velocity.linear.y = vel_start_.second.linear.y;
    start.velocity.angular.z = vel_start_.second.angular.z;
    start.time_from_start.fromSec(curr_time);

    curr_time += teb_.TimeDiff(0);

    // intermediate points
    for (int i = 1; i < n - 1; ++i)
    {
        TrajectoryPointMsg &point = trajectory[i];
        teb_.Pose(i).toPoseMsg(point.pose);
        point.velocity.linear.z = 0;
        point.velocity.angular.x = point.velocity.angular.y = 0;
        double vel1_x, vel1_y, vel2_x, vel2_y, omega1, omega2;
        extractVelocity(teb_.Pose(i - 1), teb_.Pose(i), teb_.TimeDiff(i - 1), vel1_x, vel1_y, omega1);
        extractVelocity(teb_.Pose(i), teb_.Pose(i + 1), teb_.TimeDiff(i), vel2_x, vel2_y, omega2);
        point.velocity.linear.x = 0.5 * (vel1_x + vel2_x);
        point.velocity.linear.y = 0.5 * (vel1_y + vel2_y);
        point.velocity.angular.z = 0.5 * (omega1 + omega2);
        point.time_from_start.fromSec(curr_time);

        curr_time += teb_.TimeDiff(i);
    }

    // goal
    TrajectoryPointMsg &goal = trajectory.back();
    teb_.BackPose().toPoseMsg(goal.pose);
    goal.velocity.linear.z = 0;
    goal.velocity.angular.x = goal.velocity.angular.y = 0;
    goal.velocity.linear.x = vel_goal_.second.linear.x;
    goal.velocity.linear.y = vel_goal_.second.linear.y;
    goal.velocity.angular.z = vel_goal_.second.angular.z;
    goal.time_from_start.fromSec(curr_time);
}

bool TebOptimalPlanner::isTrajectoryFeasible(base_local_planner::CostmapModel *costmap_model, const std::vector<geometry_msgs::Point> &footprint_spec,
                                             double inscribed_radius, double circumscribed_radius, int look_ahead_idx)
{
    if (look_ahead_idx < 0 || look_ahead_idx >= teb().sizePoses())
        look_ahead_idx = teb().sizePoses() - 1;

    for (int i = 0; i <= look_ahead_idx; ++i)
    {
        if (costmap_model->footprintCost(teb().Pose(i).x(), teb().Pose(i).y(), teb().Pose(i).theta(), footprint_spec, inscribed_radius, circumscribed_radius) < 0)
        {
            if (visualization_)
            {
                visualization_->publishInfeasibleRobotPose(teb().Pose(i), *robot_model_);
            }
            return false;
        }
        // Checks if the distance between two poses is higher than the robot radius or the orientation diff is bigger than the specified threshold
        // and interpolates in that case.
        // (if obstacles are pushing two consecutive poses away, the center between two consecutive poses might coincide with the obstacle ;-)!
        if (i < look_ahead_idx)
        {
            double delta_rot = g2o::normalize_theta(g2o::normalize_theta(teb().Pose(i + 1).theta()) -
                                                    g2o::normalize_theta(teb().Pose(i).theta()));
            Eigen::Vector2d delta_dist = teb().Pose(i + 1).position() - teb().Pose(i).position();

            // 轨迹的配置过于离散, 就算检测为安全轨迹, 实际中也可能发生碰撞, 因此需要插值
            if (fabs(delta_rot) > cfg_->trajectory.min_resolution_collision_check_angular || delta_dist.norm() > inscribed_radius)
            {
                int n_additional_samples = std::max(std::ceil(fabs(delta_rot) / cfg_->trajectory.min_resolution_collision_check_angular),
                                                    std::ceil(delta_dist.norm() / inscribed_radius)) - 1;
                PoseSE2 intermediate_pose = teb().Pose(i);
                for (int step = 0; step < n_additional_samples; ++step) // 在两配置点之间添加额外的配置, 使轨迹不那么离散
                {
                    // bug? step没用, 一直在添加同一个intermediate_pose
                    intermediate_pose.position() = intermediate_pose.position() + delta_dist / (n_additional_samples + 1.0);
                    intermediate_pose.theta() = g2o::normalize_theta(intermediate_pose.theta() +
                                                                     delta_rot / (n_additional_samples + 1.0));
                    if (costmap_model->footprintCost(intermediate_pose.x(), intermediate_pose.y(), intermediate_pose.theta(),
                                                     footprint_spec, inscribed_radius, circumscribed_radius) == -1) // -1 -> lethal obstacle cell
                    {
                        if (visualization_)
                        {
                            visualization_->publishInfeasibleRobotPose(intermediate_pose, *robot_model_);
                        }
                        return false;
                    }
                }
            }
        }
    }
    return true;
}

} // namespace teb_local_planner
